import cv2
import numpy as np
import glob


# 阈值
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# 棋盘格模板规格
w = 11  # 内角点个数，内角点是和其他格子连着的点
h = 8

# 世界坐标系中的棋盘格点,例如(0,0,0), (1,0,0), (2,0,0) ....,(8,5,0)，去掉Z坐标，记为二维矩阵
objp = np.zeros((w * h, 3), np.float32)
objp[:, :2] = np.mgrid[0:w, 0:h].T.reshape(-1, 2)
# 储存棋盘格角点的世界坐标和图像坐标对
objpoints = []  # 在世界坐标系中的三维点
imgpoints = []  # 在图像平面的二维点

images = glob.glob('data/calibration/*.BMP')
for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    # 找到棋盘格角点
    # 棋盘图像(8位灰度或彩色图像)         棋盘尺寸         存放角点的位置
    ret, corners = cv2.findChessboardCorners(gray, (w, h), None)
    # 如果找到足够点对，将其存储起来
    if ret == True:
        # 角点精确检测
        # 输入图像 角点初始坐标 搜索窗口为2*winsize+1 死区 求角点的迭代终止条件
        cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
        objpoints.append(objp)
        imgpoints.append(corners)
        # 将角点在图像上显示
        cv2.drawChessboardCorners(img, (w, h), corners, ret)


# 相机标定
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)

# 相机参数
f = 0.008  # 焦距, 为8mm
l = 0.0000048  # 像元大小，为4.8μm
k = 0.0000048
cx = 640  # 1280
cy = 512  # 1024
a = f / k
b = f / l
# 构造相机内参矩阵
m = np.zeros((3, 4))
m[0, 0] = a
m[1, 1] = b
m[0, 2] = cx
m[1, 2] = cy
m[2, 2] = 1

print("理论相机参数: ")
print(m[:, :3])
print("标定相机参数: ")
print(mtx)

print(dist)
